st = load(['..' filesep 'traj.mat']);
traj = st.traj;
traj.t = traj.t - min(traj.t);

ndof = 5;

figpath = [
    '..' filesep ...
    '..' filesep ...
    'docs' filesep ...
    'report' filesep ...
    'figures' filesep];

figure(129);
set(gcf, ...
    'NumberTitle', 'off', ...
    'Name', 'MEEN 652: Trajectory Tracking Comparison', ...
    'Position', [560 528 560 220], ...
    'PaperPosition', [2 3 4 1.5]);

q = data.integral_curves{end};
t = data.time{end} - min(data.time{end});

clf;
colors = [1 0 0; 0 1 0; 0 0 1; 0 1 1; 1 0 1];
lnames = cell(5, 1);

set(gca, 'ColorOrder', colors)
drawnow

for i = 1:5
    lnames{i} = ['$q_{' num2str(i) '}(t)$'];
end


hold on;
plot(q(1:5, [1:end, end])', [q(6:10, [1:end, end])', '-', ...
    'LineWidth', 2);
legend(lnames, 'interpreter', 'latex');

for i = 1:length(t);
    
end

plot(traj.x(1:ndof,:)', traj.x(ndof+1:end,:)', ':k', ...
        'LineWidth', 2);

xlabel('Hip Displacement (m)', ...
    'interpreter', 'latex', ...
    'FontSize', 12);
ylabel('System Angles (rads)', ...
    'interpreter', 'latex', ...
    'FontSize', 12);
set(gca, ...
    'Position', [.12 .21 .84 .75], ...
    'FontSize', 12);
grid on
hgexport(gcf, [figpath 'sim_angles.eps']);
